/*
*********************************************************************************************************
*
*	??????? : MODS??????. ????????????
*	??????? : modbus_slave.c
*	??    ?? : V1.5
*	?    ?? : ????
*
*	Copyright (C), 2020-2030, ?????????? www.armfly.com
*
*********************************************************************************************************
*/
#include "bsp.h"
#include "modbus_slave.h"


/*
*********************************************************************************************************
*	                                   ????????
*********************************************************************************************************
*/
static void MODS_SendWithCRC(uint8_t *_pBuf, uint8_t _ucLen);
static void MODS_SendAckOk(void);
static void MODS_SendAckErr(uint8_t _ucErrCode);

static void MODS_AnalyzeApp(void);

static void MODS_RxTimeOut(void);

static void MODS_01H(void);
static void MODS_02H(void);
static void MODS_03H(void);
static void MODS_04H(void);
static void MODS_05H(void);
static void MODS_06H(void);
static void MODS_10H(void);

static uint8_t MODS_ReadRegValue(uint16_t reg_addr, uint8_t *reg_value);
static uint8_t MODS_WriteRegValue(uint16_t reg_addr, uint16_t reg_value);

void MODS_ReciveNew(uint8_t _byte);

/*
*********************************************************************************************************
*	                                   ????
*********************************************************************************************************
*/
/*
Baud rate	Bit rate	 Bit time	 Character time	  3.5 character times
  2400	    2400 bits/s	  417 us	      4.6 ms	      16 ms
  4800	    4800 bits/s	  208 us	      2.3 ms	      8.0 ms
  9600	    9600 bits/s	  104 us	      1.2 ms	      4.0 ms
 19200	   19200 bits/s    52 us	      573 us	      2.0 ms
 38400	   38400 bits/s	   26 us	      286 us	      1.75 ms(1.0 ms)
 115200	   115200 bit/s	  8.7 us	       95 us	      1.75 ms(0.33 ms) ?????????1750us
*/
typedef struct
{
	uint32_t Bps;
	uint32_t usTimeOut;
}MODBUSBPS_T;

const MODBUSBPS_T ModbusBaudRate[] =
{	
    {2400,	16000}, /* ??????2400bps, 3.5?????????16000us */
	{4800,	 8000}, 
	{9600,	 4000},
	{19200,	 2000},
	{38400,	 1750},
	{115200, 1750},
	{128000, 1750},
	{230400, 1750},
};

static uint8_t g_mods_timeout = 0;
MODS_T g_tModS = {0};
VAR_T g_tVar;

/*
*********************************************************************************************************
*	?? ?? ??: MODS_Poll
*	???????: ?????????. ????????????????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
void MODS_Poll(void)
{
	uint16_t addr;
	uint16_t crc1;
	
	/* ????3.5????????????MODH_RxTimeOut()???????????? g_rtu_timeout = 1; ????????????? */
	if (g_mods_timeout == 0)	
	{
		return;								/* ???????????????????????? g_tModS.RxCount */
	}
	
	g_mods_timeout = 0;	 					/* ???? */

	if (g_tModS.RxCount < 4)				/* ???????????????4?????????????????8bit??+???8bit??+???????????16bit?? */
	{
		goto err_ret;
	}

	/* ????CRC???????????????????????????CRC16??????CRC16???????0???????????? */
	crc1 = CRC16_Modbus(g_tModS.RxBuf, g_tModS.RxCount);
	if (crc1 != 0)
	{
		goto err_ret;
	}

	/* ???? (1???? */
	addr = g_tModS.RxBuf[0];				/* ??1??? ??? */
	if (addr != SADDR485)		 			/* ????????????????????????? */
	{
		goto err_ret;
	}

	/* ????????????? */
	MODS_AnalyzeApp();						
	
err_ret:
	g_tModS.RxCount = 0;					/* ?????????????????????????? */
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_ReciveNew
*	???????: ????????????????????????????????????????????????????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
void MODS_ReciveNew(uint8_t _byte)
{
	/*
		3.5?????????????????????RTU??????????RTU???????????????????
		????????????????????????????Modbus?????????????????????????????????
		??????C??????
	*/
	uint8_t i;
	
	/* ??????????????????????? */
	for(i = 0; i < (sizeof(ModbusBaudRate)/sizeof(ModbusBaudRate[0])); i++)
	{
		if(SBAUD485 == ModbusBaudRate[i].Bps)
		{
			break;
		}	
	}

	g_mods_timeout = 0;
	
	/* ??????????????????us ????????1????MODBUS???, ?????2????MODBUS????*/
	bsp_StartHardTimer(1, ModbusBaudRate[i].usTimeOut, (void *)MODS_RxTimeOut);

	if (g_tModS.RxCount < S_RX_BUF_SIZE)
	{
		g_tModS.RxBuf[g_tModS.RxCount++] = _byte;
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_RxTimeOut
*	???????: ????3.5???????????????????? ?????????? g_mods_timeout = 1???????????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_RxTimeOut(void)
{
	g_mods_timeout = 1;
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_SendWithCRC
*	???????: ???????????, ??????2???CRC
*	??    ??: _pBuf ?????
*			  _ucLen ????????????CRC??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_SendWithCRC(uint8_t *_pBuf, uint8_t _ucLen)
{
	uint16_t crc;
	uint8_t buf[S_TX_BUF_SIZE];

	memcpy(buf, _pBuf, _ucLen);
	crc = CRC16_Modbus(_pBuf, _ucLen);
	buf[_ucLen++] = crc >> 8;
	buf[_ucLen++] = crc;

	RS485_SendBuf(buf, _ucLen);
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_SendAckErr
*	???????: ??????????
*	??    ??: _ucErrCode : ???????
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_SendAckErr(uint8_t _ucErrCode)
{
	uint8_t txbuf[3];

	txbuf[0] = g_tModS.RxBuf[0];					/* 485??? */
	txbuf[1] = g_tModS.RxBuf[1] | 0x80;				/* ????????? */
	txbuf[2] = _ucErrCode;							/* ???????(01,02,03,04) */

	MODS_SendWithCRC(txbuf, 3);
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_SendAckOk
*	???????: ????????????.
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_SendAckOk(void)
{
	uint8_t txbuf[6];
	uint8_t i;

	for (i = 0; i < 6; i++)
	{
		txbuf[i] = g_tModS.RxBuf[i];
	}
	MODS_SendWithCRC(txbuf, 6);
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_AnalyzeApp
*	???????: ?????????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_AnalyzeApp(void)
{
	switch (g_tModS.RxBuf[1])				/* ??2????? ?????? */
	{
		case 0x01:							/* ??????????????????led?????*/
			MODS_01H();
			bsp_PutMsg(MSG_MODS_01H, 0);	/* ???????,???????? */
			break;

		case 0x02:							/* ???????????????????*/
			MODS_02H();
			bsp_PutMsg(MSG_MODS_02H, 0);
			break;
		
		case 0x03:							/* ??????????????????????g_tVar????*/
			MODS_03H();
			bsp_PutMsg(MSG_MODS_03H, 0);
			break;
		
		case 0x04:							/* ?????????????ADC?????*/
			MODS_04H();
			bsp_PutMsg(MSG_MODS_04H, 0);
			break;
		
		case 0x05:							/* ?????????????led??*/
			MODS_05H();
			bsp_PutMsg(MSG_MODS_05H, 0);
			break;
		
		case 0x06:							/* ?????????????????????????g_tVar?????????*/
			MODS_06H();	
			bsp_PutMsg(MSG_MODS_06H, 0);
			break;
			
		case 0x10:							/* ????????????????????????g_tVar?????????*/
			MODS_10H();
			bsp_PutMsg(MSG_MODS_10H, 0);
			break;
		
		default:
			g_tModS.RspCode = RSP_ERR_CMD;
			MODS_SendAckErr(g_tModS.RspCode);	/* ??????????????? */
			break;
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_01H
*	???????: ???????????????????D01/D02/D03??
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
/* ???:??????LED????????,?????????? */
static void MODS_01H(void)
{
	/*
	 ??????
		????????:
			11 ??????
			01 ??????
			00 ????????????????
			13 ????????????????
			00 ??????????????
			25 ??????????????
			0E CRC????????
			84 CRC????????

		??????: 	1????ON??0????OFF?????????????????8???????????????????????????0????. BIT0?????1??
			11 ??????
			01 ??????
			05 ?????????
			CD ????1(???0013H-???001AH)
			6B ????2(???001BH-???0022H)
			B2 ????3(???0023H-???002AH)
			0E ????4(???0032H-???002BH)
			1B ????5(???0037H-???0033H)
			45 CRC????????
			E6 CRC????????

		????:
			01 01 10 01 00 03   29 0B	--- ???D01?????3?????????
			01 01 10 03 00 01   09 0A   --- ???D03?????????
	*/
	uint16_t reg;
	uint16_t num;
	uint16_t i;
	uint16_t m;
	uint8_t status[10];
	
	g_tModS.RspCode = RSP_OK;

	/** ??1???? ????????????????? ===============================================================*/
	/*  ????????????????????? 
		?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ CRC16
	*/
	if (g_tModS.RxCount != 8)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;				/* ?????????? */
		return;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg = BEBufToUint16(&g_tModS.RxBuf[2]); 			/* ??????? */
	num = BEBufToUint16(&g_tModS.RxBuf[4]);				/* ????????? */

	/* ??????????????????? */
	m = (num + 7) / 8;
	
	/* ???????????????????? */
	if ((reg >= REG_D01) && (num > 0) && (reg + num <= REG_DXX + 1))
	{
		for (i = 0; i < m; i++)
		{
			status[i] = 0;
		}
		
		for (i = 0; i < num; i++)
		{
			if (bsp_IsLedOn(i + 1 + reg - REG_D01))		/* ??LED??????????????????????? */
			{  
				status[i / 8] |= (1 << (i % 8));
			}
		}
	}
	else
	{
		g_tModS.RspCode = RSP_ERR_REG_ADDR;				/* ???????????? */
	}

	/** ??3???? ????? =========================================================================*/
	if (g_tModS.RspCode == RSP_OK)						/* ?????? */
	{
		g_tModS.TxCount = 0;
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[0]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[1]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = m;				 /* ????????? */

		for (i = 0; i < m; i++)
		{
			g_tModS.TxBuf[g_tModS.TxCount++] = status[i];	/* ?????????? */
		}
		MODS_SendWithCRC(g_tModS.TxBuf, g_tModS.TxCount);
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);				/* ??????????????? */
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_02H
*	???????: ??????????????K01??K03??
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_02H(void)
{
	/*
		????????:
			11 ??????
			02 ??????
			00 ?????????????
			C4 ?????????????
			00 ??????????????
			16 ??????????????
			BA CRC????????
			A9 CRC????????

		??????:  ?????????????????????????????????????????1 ????ON??0 ????OFF??
		           ?????????????LSB(??????)????????????????????????????????????????
		           ????????????????????8????????????????8????????????????????????????
		           ???????????????????8???????????????????????????????????????????????0???
			11 ??????
			02 ??????
			03 ?????????
			AC ????1(00C4H-00CBH)
			DB ????2(00CCH-00D3H)
			35 ????3(00D4H-00D9H)
			20 CRC????????
			18 CRC????????

		????:
		01 02 20 01 00 08  23CC  ---- ???T01-08????
		01 02 20 04 00 02  B3CA  ---- ???T04-05????
		01 02 20 01 00 12  A207   ---- ?? T01-18
	*/

	uint16_t reg;
	uint16_t num;
	uint16_t i;
	uint16_t m;
	uint8_t status[10];

	g_tModS.RspCode = RSP_OK;

    /** ??1???? ????????????????? ===============================================================*/
	/* ?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ CRC16 */
	if (g_tModS.RxCount != 8)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;				/* ?????????? */
		return;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg = BEBufToUint16(&g_tModS.RxBuf[2]); 			/* ??????? */
	num = BEBufToUint16(&g_tModS.RxBuf[4]);				/* ????????? */

	/* ??????????????????? */
	m = (num + 7) / 8;
	if ((reg >= REG_T01) && (num > 0) && (reg + num <= REG_TXX + 1))
	{
		for (i = 0; i < m; i++)
		{
			status[i] = 0;
		}
		for (i = 0; i < num; i++)
		{
			if (bsp_GetKeyState((KEY_ID_E)(KID_K1 + reg - REG_T01 + i)))
			{
				status[i / 8] |= (1 << (i % 8));
			}
		}
	}
	else
	{
		g_tModS.RspCode = RSP_ERR_REG_ADDR;				/* ???????????? */
	}

	/** ??3???? ????? =========================================================================*/
	if (g_tModS.RspCode == RSP_OK)						/* ?????? */
	{
		g_tModS.TxCount = 0;
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[0]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[1]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = m;				 /* ????????? */

		for (i = 0; i < m; i++)
		{
			g_tModS.TxBuf[g_tModS.TxCount++] = status[i];	/* ????T01-02?? */
		}
		MODS_SendWithCRC(g_tModS.TxBuf, g_tModS.TxCount);
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);				/* ??????????????? */
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_ReadRegValue
*	???????: ??????????????
*	??    ??: reg_addr ????????
*			  reg_value ??????????
*	?? ?? ?: 1???OK 0???????
*********************************************************************************************************
*/
static uint8_t MODS_ReadRegValue(uint16_t reg_addr, uint8_t *reg_value)
{
	uint16_t value;
	
	switch (reg_addr)									/* ??????????? */
	{
		case SLAVE_REG_P01:
			value =	g_tVar.P01;	
			break;

		case SLAVE_REG_P02:
			value =	g_tVar.P02;							/* ???????????? */
			break;
	
		default:
			return 0;									/* ???????????? 0 */
	}

	reg_value[0] = value >> 8;                          /* ???????????  */
	reg_value[1] = value;

	return 1;											/* ?????? */
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_WriteRegValue
*	???????: ?????????????
*	??    ??: reg_addr ????????
*			  reg_value ??????
*	?? ?? ?: 1???OK 0???????
*********************************************************************************************************
*/
static uint8_t MODS_WriteRegValue(uint16_t reg_addr, uint16_t reg_value)
{
	switch (reg_addr)							/* ??????????? */
	{	
		case SLAVE_REG_P01:
			g_tVar.P01 = reg_value;				/* ????????????? */
			break;
		
		case SLAVE_REG_P02:
			g_tVar.P02 = reg_value;				/* ????????????? */
			break;
		
		default:
			return 0;		/* ???????????? 0 */
	}

	return 1;		/* ?????? */
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_03H
*	???????: ??????????? ????????????????????????????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_03H(void)
{
	/*
		???????11H???????????????????006BH??????????006DH???????????????3????????????

		????????:
			11 ??????
			03 ??????
			00 ?????????????
			6B ?????????????
			00 ??????????????
			03 ??????????????
			76 CRC?????
			87 CRC?????

		??????: 	??????????????2???????????????????????????????????????????????
					??????????????????????????????????????????????????????
			11 ??????
			03 ??????
			06 ?????
			00 ????1?????(006BH)
			6B ????1?????(006BH)
			00 ????2?????(006CH)
			13 ????2 ?????(006CH)
			00 ????3?????(006DH)
			00 ????3?????(006DH)
			38 CRC?????
			B9 CRC?????

		????:
			01 03 30 06 00 01  6B0B      ---- ?? 3006H, ????????
			01 03 4000 0010 51C6         ---- ?? 4000H ??????1???????? 32???
			01 03 4001 0010 0006         ---- ?? 4001H ??????1???????? 32???

			01 03 F000 0008 770C         ---- ?? F000H ??????1???????? 16???
			01 03 F001 0008 26CC         ---- ?? F001H ??????2???????? 16???

			01 03 7000 0020 5ED2         ---- ?? 7000H ??????1???????????1?? 64???
			01 03 7001 0020 0F12         ---- ?? 7001H ??????1???????????2?? 64???

			01 03 7040 0020 5F06         ---- ?? 7040H ??????2???????????1?? 64???
	*/
	uint16_t reg;
	uint16_t num;
	uint16_t i;
	uint8_t reg_value[64];

	g_tModS.RspCode = RSP_OK;

    /** ??1???? ????????????????? ===============================================================*/
	/* ?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ CRC16 */
	if (g_tModS.RxCount != 8)								/* 03H?????????8????? */
	{
		g_tModS.RspCode = RSP_ERR_VALUE;					/* ?????????? */
		goto err_ret;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg = BEBufToUint16(&g_tModS.RxBuf[2]); 				/* ??????? */
	num = BEBufToUint16(&g_tModS.RxBuf[4]);					/* ????????? */
	
	/* ???????????????????? */
	if (num > sizeof(reg_value) / 2)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;					/* ?????????? */
		goto err_ret;
	}

	/* ????????????reg_value???? */
	for (i = 0; i < num; i++)
	{
		if (MODS_ReadRegValue(reg, &reg_value[2 * i]) == 0)	/* ??????????????reg_value???????????????????????? */
		{
			g_tModS.RspCode = RSP_ERR_REG_ADDR;				/* ???????????? */
			break;
		}
		reg++;
	}

	/** ??3???? ????? =========================================================================*/
err_ret:
	if (g_tModS.RspCode == RSP_OK)							 /* ?????? */
	{
		g_tModS.TxCount = 0;
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[0]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[1]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = num * 2;			 /* ????????? */

		for (i = 0; i < num; i++)                            /* ????????*/ 
		{
			g_tModS.TxBuf[g_tModS.TxCount++] = reg_value[2*i];
			g_tModS.TxBuf[g_tModS.TxCount++] = reg_value[2*i+1];
		}
		MODS_SendWithCRC(g_tModS.TxBuf, g_tModS.TxCount);	/* ?????????? */
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);					/* ?????????? */
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_04H
*	???????: ????????????????A01/A02?? SMA
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_04H(void)
{
	/*
		????????:
			11 ??????
			04 ??????
			00 ????????????????
			08 ????????????????
			00 ??????????????
			02 ??????????????
			F2 CRC?????
			99 CRC?????

		??????:  ?????????????2???????????????????????????????????????????????
				??????????????????????????????????????????????????????
			11 ??????
			04 ??????
			04 ?????
			00 ????1?????(0008H)
			0A ????1?????(0008H)
			00 ????2?????(0009H)
			0B ????2?????(0009H)
			8B CRC?????
			80 CRC?????

		????:

			01 04 2201 0006 2BB0  --- ?? 2201H A01???????? ?????6??????
			01 04 2201 0001 6A72  --- ?? 2201H

	*/
	uint16_t reg;
	uint16_t num;
	uint16_t i;
	uint16_t status[10];

	memset(status, 0, 20);

    /** ??1???? ????????????????? ===============================================================*/
	/* ?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ CRC16 */
	g_tModS.RspCode = RSP_OK;

	if (g_tModS.RxCount != 8)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;	/* ?????????? */
		goto err_ret;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg = BEBufToUint16(&g_tModS.RxBuf[2]); /* ??????? */
	num = BEBufToUint16(&g_tModS.RxBuf[4]);	/* ????????? */
	
	/* ??????? */
	if ((reg >= REG_A01) && (num > 0) && (reg + num <= REG_AXX + 1))
	{	
		for (i = 0; i < num; i++)
		{
			switch (reg)
			{
				/* ??????? */
				case REG_A01:
					status[i] = g_tVar.A01;
					break;
					
				default:
					status[i] = 0;
					break;
			}
			reg++;
		}
	}
	else
	{
		g_tModS.RspCode = RSP_ERR_REG_ADDR;		/* ???????????? */
	}

	/** ??3???? ????? =========================================================================*/
err_ret:
	if (g_tModS.RspCode == RSP_OK)		/* ?????? */
	{
		g_tModS.TxCount = 0;
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[0]; /* ????????? */
		g_tModS.TxBuf[g_tModS.TxCount++] = g_tModS.RxBuf[1]; /* ????????? */ 
		g_tModS.TxBuf[g_tModS.TxCount++] = num * 2;			 /* ????????? */

		for (i = 0; i < num; i++)                            /* ???????? */
		{
			g_tModS.TxBuf[g_tModS.TxCount++] = status[i] >> 8;
			g_tModS.TxBuf[g_tModS.TxCount++] = status[i] & 0xFF;
		}
		MODS_SendWithCRC(g_tModS.TxBuf, g_tModS.TxCount);   /* ?????????? */
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);	/* ??????????????? */
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_05H
*	???????: ???????????????D01/D02/D03??
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_05H(void)
{
	/*
		????????: ????????????????FF00H????????????ON????0000H????????????OFF??
		??05H???????????????????15H?????????????????????
			11 ??????
			05 ??????
			00 ?????????????
			AC ?????????????
			FF ????1?????
			00 ????2?????
			4E CRC????????
			8B CRC????????

		??????:
			11 ??????
			05 ??????
			00 ?????????????
			AC ?????????????
			FF ?????1?????
			00 ?????1?????
			4E CRC????????
			8B CRC????????

		????:
		01 05 10 01 FF 00   D93A   -- D01??
		01 05 10 01 00 00   98CA   -- D01???

		01 05 10 02 FF 00   293A   -- D02??
		01 05 10 02 00 00   68CA   -- D02???

		01 05 10 03 FF 00   78FA   -- D03??
		01 05 10 03 00 00   390A   -- D03???
	*/
	uint16_t reg;
	uint16_t value;

	g_tModS.RspCode = RSP_OK;
	
    /** ??1???? ????????????????? ===============================================================*/
	/* ?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ CRC16 */
	if (g_tModS.RxCount != 8)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;		/* ?????????? */
		goto err_ret;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg = BEBufToUint16(&g_tModS.RxBuf[2]); 	/* ??????? */
	value = BEBufToUint16(&g_tModS.RxBuf[4]);	/* ???? */
	
	if (value != 0x0000 && value != 0xFF00)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;		/* ?????????? */
		goto err_ret;
	}
	
	/* ??????? */
	if (reg == REG_D01)
	{
		g_tVar.D01 = value;
	}
	else if (reg == REG_D02)
	{
		g_tVar.D02 = value;
	}
	else if (reg == REG_D03)
	{
		g_tVar.D03 = value;
	}
	else if (reg == REG_D04)
	{
		g_tVar.D04 = value;
	}
	else
	{
		g_tModS.RspCode = RSP_ERR_REG_ADDR;		/* ???????????? */
	}
	
	/** ??3???? ????? =========================================================================*/
err_ret:
	if (g_tModS.RspCode == RSP_OK)				/* ?????? */
	{
		MODS_SendAckOk();
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);		/* ??????????????? */
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_06H
*	???????: ???????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_06H(void)
{

	/*
		???????????????06???????????????????????16?????????????????????????

		????????:
			11 ??????
			06 ??????
			00 ?????????????
			01 ?????????????
			00 ????1?????
			01 ????1?????
			9A CRC????????
			9B CRC????????

		??????:
			11 ??????
			06 ??????
			00 ?????????????
			01 ?????????????
			00 ????1?????
			01 ????1?????
			1B CRC????????
			5A	CRC????????

		????:
			01 06 30 06 00 25  A710    ---- ????????????? 2.5
			01 06 30 06 00 10  6707    ---- ????????????? 1.0


			01 06 30 1B 00 00  F6CD    ---- SMA ?????? = 0 ??????
			01 06 30 1B 00 01  370D    ---- SMA ?????? = 1
			01 06 30 1B 00 02  770C    ---- SMA ?????? = 2
			01 06 30 1B 00 05  36CE    ---- SMA ?????? = 5

			01 06 30 07 00 01  F6CB    ---- ?????????? T1
			01 06 30 07 00 02  B6CA    ---- ?????????? T2

			01 06 31 00 00 00  8736    ---- ????????????
			01 06 31 01 00 00  D6F6    ---- ????????????

*/

	uint16_t reg;
	uint16_t value;

	g_tModS.RspCode = RSP_OK;

    /** ??1???? ????????????????? ===============================================================*/
	/* ?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ CRC16 */
	if (g_tModS.RxCount != 8)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;		/* ?????????? */
		goto err_ret;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg = BEBufToUint16(&g_tModS.RxBuf[2]); 	/* ??????? */
	value = BEBufToUint16(&g_tModS.RxBuf[4]);	/* ?????? */

	if (MODS_WriteRegValue(reg, value) == 1)	/* ??????????????????????? */
	{
		;
	}
	else
	{
		g_tModS.RspCode = RSP_ERR_REG_ADDR;		/* ???????????? */
	}

	/** ??3???? ????? =========================================================================*/
err_ret:
	if (g_tModS.RspCode == RSP_OK)				/* ?????? */
	{
		MODS_SendAckOk();
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);		/* ??????????????? */
	}
}

/*
*********************************************************************************************************
*	?? ?? ??: MODS_10H
*	???????: ??????????????.  ????????????
*	??    ??: ??
*	?? ?? ?: ??
*********************************************************************************************************
*/
static void MODS_10H(void)
{
	/*
		???????11H???????????????????0001H????????????????0002H?????????2?????????
		????????0001H???????000AH??????????0002H???????0102H??

		????????:
			11 ??????
			10 ??????
			00 ????????????????
			01 ????????????????
			00 ??????????????
			02 ??????????????
			04 ?????
			00 ????1?????
			0A ????1?????
			01 ????2?????
			02 ????2?????
			C6 CRC????????
			F0 CRC????????

		??????:
			11 ??????
			06 ??????
			00 ?????????????
			01 ?????????????
			00 ????1?????
			01 ????1?????
			1B CRC????????
			5A	CRC????????

		????:
			01 10 30 00 00 06 0C  07 DE  00 0A  00 01  00 08  00 0C  00 00     389A    ---- ????? 2014-10-01 08:12:00
			01 10 30 00 00 06 0C  07 DF  00 01  00 1F  00 17  00 3B  00 39     5549    ---- ????? 2015-01-31 23:59:57

	*/
	uint16_t reg_addr;
	uint16_t reg_num;
	uint8_t byte_num;
	uint8_t i;
	uint16_t value;
	
	g_tModS.RspCode = RSP_OK;

    /** ??1???? ????????????????? ===============================================================*/
	/* ?????8bit??+???8bit??+??????????????????16bit??+???????????16bit??+ ???????8bit??+ ??????????16bit??+ CRC16 */
	if (g_tModS.RxCount < 11)
	{
		g_tModS.RspCode = RSP_ERR_VALUE;			/* ?????????? */
		goto err_ret;
	}

	/** ??2???? ??????? ===========================================================================*/
	/* ?????????????????? */
	reg_addr = BEBufToUint16(&g_tModS.RxBuf[2]); 	/* ??????? */
	reg_num = BEBufToUint16(&g_tModS.RxBuf[4]);		/* ????????? */
	byte_num = g_tModS.RxBuf[6];					/* ???????????????? */

	/* ???????????????????????????????? */
	if (byte_num != 2 * reg_num)
	{
		;
	}
	
	/* ???????? */
	for (i = 0; i < reg_num; i++)
	{
		value = BEBufToUint16(&g_tModS.RxBuf[7 + 2 * i]);	/* ?????? */

		if (MODS_WriteRegValue(reg_addr + i, value) == 1)
		{
			;
		}
		else
		{
			g_tModS.RspCode = RSP_ERR_REG_ADDR;		/* ???????????? */
			break;
		}
	}

	/** ??3???? ????? =========================================================================*/
err_ret:
	if (g_tModS.RspCode == RSP_OK)					/* ?????? */
	{
		MODS_SendAckOk();
	}
	else
	{
		MODS_SendAckErr(g_tModS.RspCode);			/* ??????????????? */
	}
}

/***************************** ?????????? www.armfly.com (END OF FILE) *********************************/
